#include "Collision_detector.h"
#include "Rrt_planner.h"
#include "Poly_util.h"
#include "Rrt_Imp.h"


#include <iostream>
#include <fstream>
#include <string>


Polygon	read_polygon(int num_vertices, std::istream* in)
{
	Polygon result;
	for (int i = 0; i < num_vertices; i++)
	{
		double x,y;
		*in >> x >> y;
		Point p(x,y,0);
		result.push_back(p);
	}

	return result;
}

Polygon	load_robot(const char* pi_szFileName)
{
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_robots;
	*in >> num_robots;
	
	// IMPORTANT:
	// ---------
	// only the first robot is read while the others are ignored

	int num_vertices;
	*in >> num_vertices;
	Polygon result =  read_polygon(num_vertices, in);
		
	ifile.close();
	return result;
}

vector<Polygon>	load_obstacles(const char* pi_szFileName)
{
	vector<Polygon> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 

	int num_obstacles;
	*in >> num_obstacles;

	if (num_obstacles != 0)
	{
		for (int i = 0; i < num_obstacles; i++)
		{
			int num_vertices;
			*in >> num_vertices;

			Polygon current =  read_polygon(num_vertices, in);
			result.push_back(current);
		}
	}
		
	ifile.close();
	return result;
}

vector<Point>	load_query(const char* pi_szFileName)
{

	vector<Point> result;
	std::ifstream ifile(pi_szFileName);
	std::istream *in = &ifile; 
		
	int num_robots;
	*in >> num_robots;

	int num_confs;
	*in >> num_confs;

	for (int i = 0; i < num_confs; i++)
	{		
		double x,y;
		double theta;
		*in >> x >> y >> theta;
		Point point(x,y,theta / 360);
		result.push_back(point);
	}
		
	ifile.close();
	return result;
}

void	save_path(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
		
	int robot_num = 1;
	fprintf(fOutput, "\n%d\n", robot_num);
		
	int size = 2*path.size()-1;
	fprintf(fOutput, "\n%d\n", size);

	if (path.size() == 0) 
	{
		fclose(fOutput);
		return; 
	}

	fprintf(fOutput, "%.5f %.5f %.5f \n", vec[0][0],vec[0][1],vec[0][2] * 360);

	for (int i = 1; i < vec.size(); i++)
	{
		double x_diff = vec[i][0] - vec[i-1][0];
		double y_diff = vec[i][1] - vec[i-1][1];
		double angle_diff = smallest_angle(vec[i-1],vec[i]);

		fprintf(fOutput, "0.00 0.00 %.5f \n", angle_diff * 360);
		fprintf(fOutput, "%.5f %.5f 0.00 \n", x_diff, y_diff);
	}
	fclose(fOutput);
}

void	save_path_MMS(const char* pi_szFileName, list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
	int size = path.size();
	fprintf(fOutput, "%d\n", size*3-2);

	if (size == 0) 
	{
		fclose(fOutput);
		return; 
	}
	double curr_angle = 0;
	for (int i = 0; i < vec.size()-1; i++)
	{
		double x = vec[i][0] ;
		double y = vec[i][1] ;
		double angle = vec[i][2];
		double angle_diff = smallest_angle(vec[i],vec[i+1]);
		curr_angle += angle_diff*360;
		
		fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i][0], vec[i][1], vec[i][2] * 360);
		fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i][0], vec[i][1],curr_angle);
		fprintf(fOutput, "%.13f %.13f %.13f \n", vec[i+1][0], vec[i+1][1],curr_angle);

	}
	fprintf(fOutput, "%.13f %.13f %.13f \n", vec[vec.size()-1][0], vec[vec.size()-1][1], vec[vec.size()-1][2] * 360);

	fclose(fOutput);
}

void	save_path_distance_angle(const char* pi_szFileName, const char* robot_num,list<Point>	path)
{
	vector<Point> vec(path.begin(), path.end());
	FILE* fOutput = fopen(pi_szFileName, "w+");
	if (!fOutput)
	{
		return;
	}
	double l_diff = 0;
	double angle_diff = 0;

	fprintf(fOutput, "id %s\n", robot_num);
		
	if (path.size() == 0) 
	{
		fclose(fOutput);
		return; 
	}

	for (int i = 1; i < vec.size(); i++)
	{
		//l_diff = distance_points(vec[i-1],vec[i]);
		l_diff = sqrt(pow((vec[i-1][0]-vec[i][0]),2)+pow((vec[i-1][1]-vec[i][1]),2));
		angle_diff = smallest_angle(vec[i-1],vec[i]);

		if(angle_diff !=0)
		{
			fprintf(fOutput, "angle %.5f\n", angle_diff * 360);
		}
		if(l_diff != 0 )
		{
			fprintf(fOutput, "line %.5f %.5f\n", vec[i][0],vec[i][1]);
		}
	}

	fclose(fOutput);
}

bool	check_polygon_simple(const char* p_fileName)
{
	Polygon points;
	Polygon_2 poly;
	bool result = false;
	std::ifstream ifile(p_fileName);
	std::istream *in = &ifile; 

	int num_vertices;

	*in >> num_vertices;
	*in >> num_vertices;

	points =  read_polygon(num_vertices, in);
	
	ifile.close();

	for(int i=0;i<points.size();i++)
	{
		poly.push_back(Point_2(points[i][0],points[i][1]));
	}

	return poly.is_simple ()?true:false;
}


int main(int argc, char *argv[])
{
	/*
		argv[1] = path number
		argv[2] = folder path
		argv[3] = step size.
		argv[4] = angle step size.
		argv[5] = itration number.
		argv[6] = connect target rate.
		argv[7] = connect line rate.
		argv[8] = bound of steps

	*/


	// CONSTANTS
	double	step_size = 0.0015; // length of each local_planner step
	double	step_angle_szie = 0.003; // length of each local_planner step
	double	line_step_size = 0.005;
	int		iteration_num = 40000;
	int		connect_target_rate = 10; 
	int		connect_line_rate = connect_target_rate*2-1;
	int		bound = 350;
	int fd_name_len = 0;	
	switch(argc)
	{
		case 8:
			connect_line_rate = atoi(argv[7]);
		case 7:
			connect_target_rate = atoi(argv[6]);
		case 6:
			iteration_num = atoi(argv[5]);
		case 5:
			step_angle_szie = atof(argv[4]);
		case 4:
			step_size = atof(argv[3]);
		case 3:
			fd_name_len = strlen(argv[2]);
	default:
		break;
	}
	
	string folder = "";
	if(fd_name_len)
	{
		folder = string(argv[2],fd_name_len);
	}

	vector<Polygon>		obstacles = load_obstacles((folder+"workspace_A.txt").c_str());
	Polygon				robot = load_robot((folder+"robot_S.txt").c_str());
	vector<Point>		query = load_query((folder+"query.txt").c_str());

	Polygon				buff_robot = poly_2_to_poly(mink_robot(robot));

	Collision_detector	col(&obstacles, robot, step_size,step_angle_szie,bound);

	Collision_detector	buff_col(&obstacles, buff_robot, step_size,step_angle_szie,bound);
	
	Rrt_planner planner(&col, &buff_col, query[0], query[1],iteration_num, connect_target_rate,connect_line_rate,line_step_size,atoi(argv[1]));
	
	list<Point> result = planner.run();

	save_path_distance_angle((folder+"path_robot.txt").c_str(),argv[1], result);
	save_path((folder+"path_R.txt").c_str(), result);
	save_path_MMS((folder+"path"+argv[1]+".txt").c_str(),result);

	return result.size()>0?1:0;
}